//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <ddk.h>
#include <drvpub.h>
#include <xllp_dmac.h>
#include <bulverde.h>
#include "ccamera.h"
#include <ddkit.h>

#define CAMDRV_BUFFERSIZE           0x200000

#define CAMDRV_PREVIEW_OFFSET       0x0
#define CAMDRV_PHOTO_OFFSET         0x071000
#define CAMDRV_PREVIEWDESC_OFFSET   0x1FD000
#define CAMDRV_PHOTODESC_OFFSET     0x1FE000

#if (_TEST_TYPE == 41)      /* Test for Runtime of ISR */
    #define SAMPLETIMES 50
	UInt32 ul_DmaChYIsrTime[SAMPLETIMES];
	UInt32 ul_DmaChCbIsrTime[SAMPLETIMES];
	UInt32 ul_DmaChCrIsrTime[SAMPLETIMES];
	Int32 n_DmaChYIsrSampleCounter = 0;
	Int32 n_DmaChCbIsrSampleCounter = 0;
	Int32 n_DmaChCrIsrSampleCounter = 0;
#endif //_TEST_TYPE == 41

static unsigned int frameCount0 = 0;
static unsigned int frameCount1 = 0;
static unsigned int frameCount2 = 0;
static DzEvent StillSnapEvent;
static DzEvent FrameCopyEvent;

#define DMA_CHANNEL0_FINISH 0x1
#define DMA_CHANNEL1_FINISH 0x2
#define DMA_CHANNEL2_FINISH 0x4
#define DMA_ALL_CHANNELS_FINISH \
    (DMA_CHANNEL0_FINISH | DMA_CHANNEL1_FINISH | DMA_CHANNEL2_FINISH)
static unsigned long StillDmaTransFlag = 0;

typedef ECode (*CamDrv_SetFormatFunc)(PCamDrv_Context pCameraCxt, int param);
CamDrv_SetFormatFunc s_fpFormatSet[] = {
    CamDrv_SetPhotoResolution,
    NULL,
    NULL,
    CamDrv_SetDigitalZoom,
    CamDrv_SetBrightness,
    CamDrv_SetContrast,
    CamDrv_SetSharpness,
    NULL,
    CamDrv_SetColorMode,
    CamDrv_SetSceneMode,
    NULL,
    CamDrv_SetFlash,
    NULL,
    NULL,
    CamDrv_SetContinuousSnap
    };

//extern I2C function
EXTERN_C void I2C_Initial(void);
EXTERN_C void I2C_Disable();

EXTERN_C void CIDumpRegisters();
EXTERN_C OverlayFormat *GetDisplayOverlayFormat(int layer);


// There are 22 WMMX registers should be stored:
// 16 * 64bits Data Registers
// 6 * 32bits Control Registers
#define MMX_SIZE                (0x98)
EXTERN_C unsigned long *g_pMmxData;
unsigned long *g_pMmxData = NULL;
static unsigned char s_mmxData[MMX_SIZE + 4];


#ifdef DEBUG_CAMERA
static void DMADumpBufferInfo(PCamDrv_Context pCameraCxt, Boolean bPreview)
{
    unsigned int i, j;
    PCamDrv_DMABuffer pCameraBuffer;

    if (bPreview) {
        pCameraBuffer = &pCameraCxt->PreviewBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->PhotoBuffer;
    }

    kprintf("\nDump DMA buffer info for camera:\n");
    kprintf("Buffer address:\n");
    for (i = 0; i < pCameraCxt->UsedFrames; i++) {
        for (j = 0; j < TRANS_CHANNELS; j++) {
            kprintf("(Frame %d, Channel %d)Physical: 0x%08x, Virtual: 0x%08x\n",
                i, j, pCameraBuffer->BufPhyAddr[i][j],
                pCameraBuffer->BufVirAddrKernel[i][j]);
        }
    }
    kprintf("Descriptor address:\n");
    for (j = 0; j < TRANS_CHANNELS; j++) {
        kprintf("(Channel %d)Physical: 0x%08x, Virtual: 0x%08x\n",
            j, pCameraBuffer->DescPhyAddr[j], pCameraBuffer->DescVirAddr[j]);
    }
    kprintf("FrameBufSize & FrameDescNum:\n");
    for (j = 0; j < TRANS_CHANNELS; j++) {
        kprintf("(Channel %d) FrameBufSize: %d, FrameDescNum: %d\n",
            j, pCameraBuffer->FrameBufSize[j], pCameraBuffer->FrameDescNum[j]);
    }
    kprintf("\n\n");


}

static void DMADumpDescriptors(PCamDrv_Context pCameraCxt, Boolean bPreview)
{
    unsigned int i, j, k;
    XLLP_DMAC_DESCRIPTOR_T *CurDesVirtual, *CurDesPhysical;
    PCamDrv_DMABuffer pCameraBuffer;

    if (bPreview) {
        pCameraBuffer = &pCameraCxt->PreviewBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->PhotoBuffer;
    }

    for (i = 0; i < TRANS_CHANNELS; i++) {
        CurDesVirtual = (XLLP_DMAC_DESCRIPTOR_T *)pCameraBuffer->DescVirAddr[i];
        CurDesPhysical = (XLLP_DMAC_DESCRIPTOR_T *)pCameraBuffer->DescPhyAddr[i];
        for (j = 0; j < pCameraCxt->UsedFrames; j++) {
            for(k = 0; k < pCameraBuffer->FrameDescNum[i]; k++) {
                kprintf("DMA Descriptor 0x%08x"\
                    "(Channel %d, Frame %d, Index %d):\n",
                    CurDesPhysical, i, j, k);
                kprintf("DDADR: 0x%08x\n", CurDesVirtual->DDADR);
                kprintf("DSADR: 0x%08x\n", CurDesVirtual->DSADR);
                kprintf("DTADR: 0x%08x\n", CurDesVirtual->DTADR);
                kprintf("DCMD: 0x%08x\n", CurDesVirtual->DCMD);
                kprintf("\n");
                CurDesPhysical++;
                CurDesVirtual++;
            }
        }
    }

}
#endif

static void DMAFinishOneFrameStillCapture(PCamDrv_Context pCameraCxt)
{
    if (StillDmaTransFlag == DMA_ALL_CHANNELS_FINISH) {
        PCamDrv_DMABuffer pCameraBuffer = &pCameraCxt->PhotoBuffer;

        DBGOUT(kprintf("DMAFinishOneFrameStillCapture\n"));

        StillDmaTransFlag = 0;

        IppYCbCr422ResizeCopy(
            (unsigned char *)pCameraBuffer->BufVirAddrKernel[frameCount0][0],
            (unsigned char *)pCameraBuffer->BufVirAddrKernel[frameCount1][1],
            (unsigned char *)pCameraBuffer->BufVirAddrKernel[frameCount2][2],
            (unsigned char *)pCameraCxt->pOverlay2->pChannelFb[0], // ChanY
            (unsigned char *)pCameraCxt->pOverlay2->pChannelFb[1], // ChanCb
            (unsigned char *)pCameraCxt->pOverlay2->pChannelFb[2], // ChanCr
            pCameraCxt->DrvResolution.PhotoWidth,
            pCameraCxt->DrvResolution.PhotoHeight,
            pCameraCxt->pOverlay2->Width, pCameraCxt->pOverlay2->Height);

        frameCount0++;
        frameCount1++;
        frameCount2++;

        SensorAfterStillCapture();
        if (frameCount0 == pCameraCxt->UsedFrames) {
            // stop dma
            DMAStopTransfer(pCameraCxt);

            CIStopStillCapture();
            StillSnapEvent.NotifyByIsr();
        }
        else {
            SensorBeforeStillCapture();
        }

    }
}

static void DMAFinishOneFrameVideoCapture(PCamDrv_Context pCameraCxt)
{
    if (StillDmaTransFlag == DMA_ALL_CHANNELS_FINISH) {
        PCamDrv_DMABuffer pCameraBuffer = &pCameraCxt->PreviewBuffer;

//        DBGOUT(kprintf("DMAFinishOneFrameVideoCapture\n"));
        StillDmaTransFlag = 0;

        IppYCbCr422ResizeCopy(
            (unsigned char *)pCameraBuffer->BufVirAddrKernel[frameCount0][0],
            (unsigned char *)pCameraBuffer->BufVirAddrKernel[frameCount1][1],
            (unsigned char *)pCameraBuffer->BufVirAddrKernel[frameCount2][2],
            (unsigned char *)pCameraCxt->pOverlay2->pChannelFb[0], // ChanY
            (unsigned char *)pCameraCxt->pOverlay2->pChannelFb[1], // ChanCb
            (unsigned char *)pCameraCxt->pOverlay2->pChannelFb[2], // ChanCr
            pCameraCxt->DrvResolution.VideoWidth,
            pCameraCxt->DrvResolution.VideoHeight,
            pCameraCxt->pOverlay2->Width, pCameraCxt->pOverlay2->Height);

        frameCount0 = (frameCount0 + 1) % pCameraCxt->UsedFrames;
        frameCount1 = (frameCount1 + 1) % pCameraCxt->UsedFrames;
        frameCount2 = (frameCount2 + 1) % pCameraCxt->UsedFrames;
    }
}

static Boolean _DmaChYIsr(PVoid pDev, PBoolean pWakeUp)
{
#if (_TEST_TYPE == 41)
    UInt32 uTimerStart = OSCR0;
#endif //_TEST_TYPE == 41
    PCamDrv_Context pCameraCxt = (PCamDrv_Context)pDev;
    unsigned long nDmaChY = pCameraCxt->DMAChannels[0];

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if(DCSR(nDmaChY) & XLLP_DMAC_DCSR_END_INTR) {
        DCSR(nDmaChY) |= XLLP_DMAC_DCSR_END_INTR;

//        DBGOUT(kprintf("_DmaChYIsr\n"));
        StillDmaTransFlag |= DMA_CHANNEL0_FINISH;
        if (pCameraCxt->CaptureStatus == CamDrv_Status_Still ) {
            if (frameCount0 <= pCameraCxt->UsedFrames) {
//                StillDmaTransFlag |= DMA_CHANNEL0_FINISH;
                DMAFinishOneFrameStillCapture(pCameraCxt);
            }
        }
        else {
            DMAFinishOneFrameVideoCapture(pCameraCxt);
       }

    }
    else if (DCSR(nDmaChY) & XLLP_DMAC_DCSR_BUS_ERR_INTR) {
        DCSR(nDmaChY) |= XLLP_DMAC_DCSR_BUS_ERR_INTR;
    }

#if (_TEST_TYPE == 41)
    UInt32 uTimerEnd = OSCR0;
    if (n_DmaChYIsrSampleCounter >= SAMPLETIMES) n_DmaChYIsrSampleCounter = 0;
    ul_DmaChYIsrTime[n_DmaChYIsrSampleCounter++] = \
                            (uTimerEnd >= uTimerStart) ? \
                            (uTimerEnd - uTimerStart) : \
                            ((UInt32)0xFFFFFFFF - uTimerStart + uTimerEnd);
#endif //_TEST_TYPE == 41
    return TRUE;
}

static Boolean _DmaChCbIsr(PVoid pDev, PBoolean pWakeUp)
{
#if (_TEST_TYPE == 41)
    UInt32 uTimerStart = OSCR0;
#endif //_TEST_TYPE == 41
    PCamDrv_Context pCameraCxt = (PCamDrv_Context)pDev;
    unsigned long nDmaChCb = pCameraCxt->DMAChannels[1];

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if(DCSR(nDmaChCb) & XLLP_DMAC_DCSR_END_INTR) {
        DCSR(nDmaChCb) |= XLLP_DMAC_DCSR_END_INTR;

//        DBGOUT(kprintf("_DmaChCbIsr\n"));
        StillDmaTransFlag |= DMA_CHANNEL1_FINISH;
        if (pCameraCxt->CaptureStatus == CamDrv_Status_Still ) {
            if (frameCount1 <= pCameraCxt->UsedFrames) {
                DMAFinishOneFrameStillCapture(pCameraCxt);
            }
        }
        else {
            DMAFinishOneFrameVideoCapture(pCameraCxt);
        }
    }
    else if (DCSR(nDmaChCb) & XLLP_DMAC_DCSR_BUS_ERR_INTR) {
        DCSR(nDmaChCb) |= XLLP_DMAC_DCSR_BUS_ERR_INTR;
    }
#if (_TEST_TYPE == 41)
    UInt32 uTimerEnd = OSCR0;
    if (n_DmaChCbIsrSampleCounter >= SAMPLETIMES) n_DmaChCbIsrSampleCounter = 0;
    ul_DmaChCbIsrTime[n_DmaChCbIsrSampleCounter++] = \
                            (uTimerEnd >= uTimerStart) ? \
                            (uTimerEnd - uTimerStart) : \
                            ((UInt32)0xFFFFFFFF - uTimerStart + uTimerEnd);
#endif //_TEST_TYPE == 41
    return TRUE;
}

static Boolean _DmaChCrIsr(PVoid pDev, PBoolean pWakeUp)
{
#if (_TEST_TYPE == 41)
    UInt32 uTimerStart = OSCR0;
#endif //_TEST_TYPE == 41
    PCamDrv_Context pCameraCxt = (PCamDrv_Context)pDev;
    unsigned long nDmaChCr = pCameraCxt->DMAChannels[2];

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if(DCSR(nDmaChCr) & XLLP_DMAC_DCSR_END_INTR) {
        DCSR(nDmaChCr) |= XLLP_DMAC_DCSR_END_INTR;

//        DBGOUT(kprintf("_DmaChCrIsr\n"));
        StillDmaTransFlag |= DMA_CHANNEL2_FINISH;
        if (pCameraCxt->CaptureStatus == CamDrv_Status_Still ) {
            if (frameCount2 <= pCameraCxt->UsedFrames) {
                DMAFinishOneFrameStillCapture(pCameraCxt);
            }
        }
        else {
            DMAFinishOneFrameVideoCapture(pCameraCxt);
        }
    }
    else if (DCSR(nDmaChCr) & XLLP_DMAC_DCSR_BUS_ERR_INTR) {
        DCSR(nDmaChCr) |= XLLP_DMAC_DCSR_BUS_ERR_INTR;
    }
#if (_TEST_TYPE == 41)
    UInt32 uTimerEnd = OSCR0;
    if (n_DmaChCrIsrSampleCounter >= SAMPLETIMES) n_DmaChCrIsrSampleCounter = 0;
    ul_DmaChCrIsrTime[n_DmaChCrIsrSampleCounter++] = \
                            (uTimerEnd >= uTimerStart) ? \
                            (uTimerEnd - uTimerStart) : \
                            ((UInt32)0xFFFFFFFF - uTimerStart + uTimerEnd);
#endif //_TEST_TYPE == 41
    return TRUE;
}

EXTERN_C void DMAStartTransfer(PCamDrv_Context pCameraCxt)
{
    unsigned int i;
    PCamDrv_DMABuffer pCameraBuffer;

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        pCameraBuffer = &pCameraCxt->PreviewBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->PhotoBuffer;
    }

    frameCount0 = 0;
    frameCount1 = 0;
    frameCount2 = 0;

    DRCMRCI0 = XLLP_DMAC_DRCMR_ENABLE | pCameraCxt->DMAChannels[0];
    DRCMRCI1 = XLLP_DMAC_DRCMR_ENABLE | pCameraCxt->DMAChannels[1];
    DRCMRCI2 = XLLP_DMAC_DRCMR_ENABLE | pCameraCxt->DMAChannels[2];

    // start channel 0 ~ 2
    for (i = 0; i < TRANS_CHANNELS; i++) {
        DDADR(pCameraCxt->DMAChannels[i]) = pCameraBuffer->DescPhyAddr[i];
        DCSR(pCameraCxt->DMAChannels[i]) |= XLLP_DMAC_DCSR_RUN;
    }

}

EXTERN_C void DMAStopTransfer(PCamDrv_Context pCameraCxt)
{
    frameCount0 = 0;
    frameCount1 = 0;
    frameCount2 = 0;

    DCSR(pCameraCxt->DMAChannels[0]) &= ~XLLP_DMAC_DCSR_RUN;
    DCSR(pCameraCxt->DMAChannels[1]) &= ~XLLP_DMAC_DCSR_RUN;
    DCSR(pCameraCxt->DMAChannels[2]) &= ~XLLP_DMAC_DCSR_RUN;

}

EXTERN_C void DMAAllocChannel(PCamDrv_Context pCameraCxt)
{
        //init context status
    pCameraCxt->DMAChannels[0] = Drv_DmaInstall(0, (void *)pCameraCxt,
        (void *)_DmaChYIsr);
    pCameraCxt->DMAChannels[1] = Drv_DmaInstall(0, (void *)pCameraCxt,
        (void *)_DmaChCbIsr);
    pCameraCxt->DMAChannels[2] = Drv_DmaInstall(0, (void *)pCameraCxt,
        (void *)_DmaChCrIsr);

    kprintf("DMA Channels: Y %d, Cb %d, Cr %d\n",
         pCameraCxt->DMAChannels[0],
         pCameraCxt->DMAChannels[1],
         pCameraCxt->DMAChannels[2]);

}

static void DMAUpdateChannelChain(PCamDrv_Context pCameraCxt,
    unsigned int ChanIdx, unsigned long FifoPhyAddr, unsigned long DmaBurstSize)
{
    XLLP_DMAC_DESCRIPTOR_T *CurDesVirtual, *CurDesPhysical;
    XLLP_DMAC_DESCRIPTOR_T *LastDesVirtual = NULL;
    int DesTransferSize = 0, RemainSize;
	unsigned int i, j;
    unsigned long TargetPhysical;
    PCamDrv_DMABuffer pCameraBuffer;

    Drv_Assert(pCameraCxt->CaptureStatus == CamDrv_Status_Preview
        || pCameraCxt->CaptureStatus == CamDrv_Status_Still);

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        pCameraBuffer = &pCameraCxt->PreviewBuffer;
    }
    else{
        pCameraBuffer = &pCameraCxt->PhotoBuffer;
    }

    // generate fifo0 dma chains
    CurDesVirtual = (XLLP_DMAC_DESCRIPTOR_T*)
        pCameraBuffer->DescVirAddr[ChanIdx];
    CurDesPhysical = (XLLP_DMAC_DESCRIPTOR_T*)
        pCameraBuffer->DescPhyAddr[ChanIdx];

    for(i = 0; i < pCameraCxt->UsedFrames; i++) {
        // in each iteration, generate one dma chain for one frame
        RemainSize = pCameraBuffer->FrameBufSize[ChanIdx];

        // assume the blocks are stored consecutively
        TargetPhysical = pCameraBuffer->BufPhyAddr[i][ChanIdx];

        for(j = 0; j < pCameraBuffer->FrameDescNum[ChanIdx]; j++) {
            // set descriptor
            if (RemainSize > SINGLE_DESCRIPTOR_TRANSFER_MAX)
                DesTransferSize = SINGLE_DESCRIPTOR_TRANSFER_MAX;
            else
                DesTransferSize = RemainSize;
            CurDesVirtual->DDADR = (unsigned long)CurDesPhysical +
                sizeof(XLLP_DMAC_DESCRIPTOR_T);

            CurDesVirtual->DSADR = FifoPhyAddr; // FIFO0~FIFO2 physical address
            CurDesVirtual->DTADR = TargetPhysical;
            CurDesVirtual->DCMD = DesTransferSize | XLLP_DMAC_DCMD_FLOW_SRC |
                     XLLP_DMAC_DCMD_INC_TRG_ADDR | (DmaBurstSize << 16);

            // advance pointers
            RemainSize -= DesTransferSize;
            CurDesPhysical++;
            CurDesVirtual++;
            TargetPhysical += DesTransferSize;

        }

        // stop the dma transfer on one frame captured
        LastDesVirtual = CurDesVirtual - 1;
        LastDesVirtual->DCMD = DesTransferSize |XLLP_DMAC_DCMD_END_IRQ_EN |
            XLLP_DMAC_DCMD_FLOW_SRC | XLLP_DMAC_DCMD_INC_TRG_ADDR |
            (DmaBurstSize << 16);
    }

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        LastDesVirtual->DDADR =
            ((unsigned int)pCameraBuffer->DescPhyAddr[ChanIdx]);
    }
    else {
        LastDesVirtual->DDADR = 0;
    }
}

// Fill DMA descriptors
static void DMAUpdateChain(PCamDrv_Context pCameraCxt)
{
    DMAUpdateChannelChain(pCameraCxt, 0, CIBR0_PHY, XLLP_DMAC_BURSTSIZE_16);
    DMAUpdateChannelChain(pCameraCxt, 1, CIBR1_PHY, XLLP_DMAC_BURSTSIZE_8);
    DMAUpdateChannelChain(pCameraCxt, 2, CIBR2_PHY, XLLP_DMAC_BURSTSIZE_8);
//    DMADumpDescriptors(pCameraCxt);
}

EXTERN_C ECode DMAAllocBuffer(PCamDrv_Context pCameraCxt, Boolean bPreview)
{
    unsigned int i;
    unsigned int CaptureWidth = 0, CaptureHeight = 0;
    unsigned int AllocBufSize, AllocDescSize, FrameBufSize;
    unsigned int BufPhyAddr, BufVirAddrKernel, BufVirAddrUser;
    unsigned int DescPhyAddr, DescVirAddr;
    CamDrv_DMABuffer *pCameraBuffer;
    ECode ec;

    if (bPreview) {
        pCameraBuffer = &pCameraCxt->PreviewBuffer;
        CaptureWidth = pCameraCxt->DrvResolution.VideoWidth;
        CaptureHeight = pCameraCxt->DrvResolution.VideoHeight;
    }
    else {
        pCameraBuffer = &pCameraCxt->PhotoBuffer;
        CaptureWidth = pCameraCxt->DrvResolution.PhotoWidth;
        CaptureHeight = pCameraCxt->DrvResolution.PhotoHeight;
    }

    // MUST USE YCbCr422
    FrameBufSize = CaptureWidth * CaptureHeight * 2;
    AllocBufSize = RoundUp2(FrameBufSize * pCameraCxt->UsedFrames, PAGE_SIZE);

    // Alloc memory for DMA buffer
    BufVirAddrKernel = (unsigned int)Drv_AllocBuffer(AllocBufSize >> PAGE_SHIFT, 0);
    if (!BufVirAddrKernel) {
        kprintf("Alloc camera buffer error!\n");
        return E_OUT_OF_MEMORY;
    }
    BufPhyAddr = Drv_GetBufferPhysicalAddress((void *)BufVirAddrKernel);

    ec = DrvIoRemap(0, AllocBufSize, BufPhyAddr,
            0, (Address *)&BufVirAddrUser);
    if (FAILED(ec)) return ec;

//    memset((void *)BufVirAddrKernel, 0, AllocBufSize);

    pCameraBuffer->AllocBufSize = AllocBufSize;
    pCameraBuffer->FrameBufSize[0] = FrameBufSize / 2;
    pCameraBuffer->FrameBufSize[1] = FrameBufSize / 4;
    pCameraBuffer->FrameBufSize[2] = FrameBufSize / 4;

    for (i = 0; i < pCameraCxt->UsedFrames; i++) {
        pCameraBuffer->BufVirAddrUser[i][0] = BufVirAddrUser +
                    i * FrameBufSize;
        pCameraBuffer->BufVirAddrKernel[i][0] = BufVirAddrKernel +
                    i * FrameBufSize;
        pCameraBuffer->BufPhyAddr[i][0] = BufPhyAddr +
                    i * FrameBufSize;


        pCameraBuffer->BufVirAddrUser[i][1] =
                    pCameraBuffer->BufVirAddrUser[i][0] +
                    pCameraBuffer->FrameBufSize[0];
        pCameraBuffer->BufVirAddrKernel[i][1] =
                    pCameraBuffer->BufVirAddrKernel[i][0] +
                    pCameraBuffer->FrameBufSize[0];
        pCameraBuffer->BufPhyAddr[i][1] =
                    pCameraBuffer->BufPhyAddr[i][0] +
                    pCameraBuffer->FrameBufSize[0];


        pCameraBuffer->BufVirAddrUser[i][2] =
                    pCameraBuffer->BufVirAddrUser[i][1] +
                    pCameraBuffer->FrameBufSize[1];
        pCameraBuffer->BufVirAddrKernel[i][2] =
                    pCameraBuffer->BufVirAddrKernel[i][1] +
                    pCameraBuffer->FrameBufSize[1];
        pCameraBuffer->BufPhyAddr[i][2] =
                    pCameraBuffer->BufPhyAddr[i][1] +
                    pCameraBuffer->FrameBufSize[1];
    }

    AllocDescSize = 0;
    for (i = 0; i < TRANS_CHANNELS; i++) {
        pCameraBuffer->FrameDescNum[i] =
                (pCameraBuffer->FrameBufSize[i] +
                SINGLE_DESCRIPTOR_TRANSFER_MAX - 1)
                / SINGLE_DESCRIPTOR_TRANSFER_MAX;
        AllocDescSize += pCameraBuffer->FrameDescNum[i] *
                pCameraCxt->UsedFrames * 16;
    }
    AllocDescSize = RoundUp2(AllocDescSize, PAGE_SIZE);

    // Alloc memory for DMA Descriptors
    DescVirAddr = (unsigned int)Drv_AllocBuffer(
            AllocDescSize >> PAGE_SHIFT, DRVBUF_SPEED_FAST);
    if (!DescVirAddr) {
        kprintf("Alloc camera descriptors error!\n");
        return E_OUT_OF_MEMORY;
    }
    DescPhyAddr = Drv_GetBufferPhysicalAddress((void *)DescVirAddr);

    memset((void *)DescVirAddr, 0, AllocDescSize);

    pCameraBuffer->AllocDescSize = AllocDescSize;
    pCameraBuffer->DescPhyAddr[0] = DescPhyAddr;
    pCameraBuffer->DescVirAddr[0] = DescVirAddr;
    pCameraBuffer->DescPhyAddr[1] = DescPhyAddr +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[0] * 16;
    pCameraBuffer->DescVirAddr[1] = DescVirAddr +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[0] * 16;
    pCameraBuffer->DescPhyAddr[2] = pCameraBuffer->DescPhyAddr[1] +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[1] * 16;
    pCameraBuffer->DescVirAddr[2] = pCameraBuffer->DescVirAddr[1] +
        pCameraCxt->UsedFrames * pCameraBuffer->FrameDescNum[1] * 16;

//    DMADumpBufferInfo(pCameraCxt, TRUE);

    DMAUpdateChain(pCameraCxt);

    return NOERROR;
}

EXTERN_C void DMAFreeBuffer(PCamDrv_Context pCameraCxt, Boolean bPreview)
{
    CamDrv_DMABuffer *pCameraBuffer;

    if (bPreview) {
        pCameraBuffer = &pCameraCxt->PreviewBuffer;
    }
    else {
        pCameraBuffer = &pCameraCxt->PhotoBuffer;
    }

    DrvIoUnmap(pCameraBuffer->BufVirAddrUser[0][0], pCameraBuffer->AllocBufSize);

    Drv_FreeBuffer((void *)pCameraBuffer->DescVirAddr[0],
        pCameraBuffer->AllocDescSize >> PAGE_SHIFT);
    Drv_FreeBuffer((void *)pCameraBuffer->BufVirAddrKernel[0][0],
        pCameraBuffer->AllocBufSize >> PAGE_SHIFT);

    // Tag for the free memory
    pCameraBuffer->AllocBufSize = 0;
    pCameraBuffer->AllocDescSize = 0;
}

#if 0
// If no DMA buffer to use, return FALSE
static Boolean DMACheckMemoryAvailable(PCamDrv_Context pCameraCxt)
{
    if ((void *)pCameraCxt->pCameraBuffer->DescVirAddr[0] == NULL ||
        (void *)pCameraCxt->pCameraBuffer->BufVirAddr[0][0] == NULL)
        return FALSE;
    return TRUE;
}
#endif

#ifdef DEBUG_CAMERA
static void CamDrv_DumpCurrentFormat(CamDrv_AppFormat *pFormat)
{
    kprintf("Dump current format information of camera:\n");
    kprintf("PhotoResolution       = %d\n",  pFormat->PhotoResolution);
    kprintf("VideoResolution       = %d\n",  pFormat->VideoResolution);
    kprintf("FrameRate             = %d\n",  pFormat->FrameRate);
    kprintf("Zoom                  = %d\n",  pFormat->Zoom);
    kprintf("Brightness            = %d\n",  pFormat->Brightness);
    kprintf("Contrast              = %d\n",  pFormat->Contrast);
    kprintf("Sharpness             = %d\n",  pFormat->Sharpness);
    kprintf("WhiteBalance          = %d\n",  pFormat->WhiteBalance);
    kprintf("ColorMode             = %d\n",  pFormat->ColorMode);
    kprintf("SceneMode             = %d\n",  pFormat->SceneMode);
    kprintf("Direction             = %d\n",  pFormat->Direction);
    kprintf("Flash                 = %d\n",  pFormat->Flash);
    kprintf("ISO                   = %d\n",  pFormat->ISO);
    kprintf("ExposureCompensation  = %d\n",  pFormat->ExposureCompensation);
    kprintf("\n");
}

static void CamDrv_DumpPhotoInfo(Camera_Photo* pPhoto)
{
    kprintf("The snapped photo info\n");
    kprintf("Number            = %d\n", pPhoto->Number);
    kprintf("SingleBufferSize  = %d\n", pPhoto->SingleBufferSize);
    kprintf("pBuffer           = 0x%x\n", pPhoto->pBuffer);
    kprintf("\n");
}
#endif


static Boolean CamDrv_PreCapture()
{
    Boolean bPreemption = DzDisablePreemption();

    return bPreemption;
}

static void CamDrv_PostCapture(Boolean bPreemption)
{
    // Enable interrupt of pen and perform a dummy read so that
    // the interrupt of pen can come again.

    DzRestorePreemption(bPreemption);
}


EXTERN_C ECode CamDrv_StartStillCapture(PCamDrv_Context pCameraCxt)
{
    ECode ec = NOERROR;

    // Get overlay2 information
    pCameraCxt->pOverlay2 = GetDisplayOverlayFormat(2);
    if (!pCameraCxt->pOverlay2->Enable) {
        kprintf("Overlay2 isn't set, camera should not be opened!\n");
        return E_ACCESS_DENIED;
    }

//    if (!DMACheckMemoryAvailable(pCameraCxt)) return E_OUT_OF_MEMORY;
    pCameraCxt->UsedFrames = pCameraCxt->AppFormat.ContinuousSnap;

    if (pCameraCxt->PhotoBuffer.AllocBufSize == 0) {
        ec = DMAAllocBuffer(pCameraCxt, FALSE);
        if (FAILED(ec)) return ec;
//      DMADumpDescriptors(pCameraCxt);
    }

    if (pCameraCxt->AppFormat.Flash != CAMERA_FL_CLOSED) {
        GpioOpenFlash();
    }

    Boolean bPreemption = CamDrv_PreCapture();

    CIPrepare(pCameraCxt);
    SensorBeforeStillCapture();
    CICaptureStillImage();
    DMAStartTransfer(pCameraCxt);

    CamDrv_PostCapture(bPreemption);

    WaitResult wr;
    StillSnapEvent.TryWait(MAX_WAIT_TIME, &wr, NULL);
    if (WaitResult_TimedOut == wr) {
        // No buffers to use after waiting, perhaps the device isn't open.
        DBGOUT(kprintf("Time is out when getting still photo.\n"));
        ec = E_TIMED_OUT;
        return ec;
    }
    if (pCameraCxt->AppFormat.Flash != CAMERA_FL_CLOSED) {
        GpioCloseFlash();
    }

    return ec;

}

EXTERN_C ECode CamDrv_StartVideoCapture(PCamDrv_Context pCameraCxt)
{
    ECode ec = NOERROR;

    // Get overlay2 information
    pCameraCxt->pOverlay2 = GetDisplayOverlayFormat(2);
    if (!pCameraCxt->pOverlay2->Enable) {
        kprintf("Overlay2 isn't set, camera should not be opened!\n");
        return E_ACCESS_DENIED;
    }

    if (pCameraCxt->DrvResolution.VideoWidth < 8 ||
        pCameraCxt->DrvResolution.VideoHeight < 8) {
        return E_INVALID_ARGUMENT; // Preview window too small!
    }

    pCameraCxt->UsedFrames = DEFAULT_FRAMES;

    // Note: Preview buffer should not be changed unless driver
    // preview resolution is changed.
    if (pCameraCxt->PreviewBuffer.AllocBufSize == 0) {
        ec = DMAAllocBuffer(pCameraCxt, TRUE);
        if (FAILED(ec)) return ec;
    }


    Boolean bPreemption = CamDrv_PreCapture();

    CIPrepare(pCameraCxt);
	CIStartVideoCapture();

	// start dma
    DMAStartTransfer(pCameraCxt);

    CamDrv_PostCapture(bPreemption);

//    CIDumpRegisters();

    return ec;
}

EXTERN_C void CamDrv_StopVideoCapture(PCamDrv_Context pCameraCxt)
{
    DMAStopTransfer(pCameraCxt);
	CIStopVideoCapture();
}

EXTERN_C void CamDrv_ReleaseFrames(PCamDrv_Context pCameraCxt)
{
    // Free photo buffer
    DMAFreeBuffer(pCameraCxt, FALSE);
}

// Invoke this function when camera driver is loading
EXTERN_C ECode CamDrv_Install(PCamDrv_Context pCameraCxt)
{
    DMAAllocChannel(pCameraCxt);

	return NOERROR;
}

EXTERN_C ECode CamDrv_OpenHardware(PCamDrv_Context pCameraCxt)
{
    //initialize the I2C
    I2C_Initial();

    //config camera GPIO
    GpioSetup();

	//capture interface init
    CIInit();

    // Configure CI according to sensor's hardware
    // master parallel with 8 data pins
    CISetMode(XLLP_CI_MODE_MP/*0*/, XLLP_CI_DATA_WIDTH8/*0x2*/);

    // enable pixel clock(sensor will provide pclock) and master clock = 24MHZ
    CISetClock(1, 1, 1300); // 24MHz

    // data sample on rising and h,vsync active high
    CISetPolarity(0, 0, 0);

    // fifo control, XLLP_CI_FIFO_THL_32 -> 32 bytes or
    // more in FIFO_0 causes a DMA request.
    CISetFIFO(1000/*time out*/, XLLP_CI_FIFO_THL_32, 1, 1);

    //CMOS Image Signal Processor Init through I2C bus
    SensorInit();
//    SensorSetDigitalZoom(1);

    //enable dma interrupt

    DBGOUT(kprintf("open dma interrupt\n"));

	CISetInterruptMask(
		XLLP_CI_CICR0_FOM |
        XLLP_CI_CICR0_EOFM |
        XLLP_CI_CICR0_SOFM |
        XLLP_CI_CICR0_CDM |
        XLLP_CI_CICR0_QDM |
        XLLP_CI_CICR0_PERRM |
        XLLP_CI_CICR0_EOLM |
        XLLP_CI_CICR0_FEM );

	 // Adjust output image (horizontal & vertical mirror)
    SensorAdjustImage();

    // s_pMmxData should be 8 bytes aligned
    g_pMmxData = (ulong_t *)(((unsigned long)s_mmxData + 4) & ~7);

    return NOERROR;
}

EXTERN_C void CamDrv_CloseHardware(PCamDrv_Context pCameraCxt)
{
    CIDeInit();

    GpioCloseCamera();

    // Close I2C
    I2C_Disable();
}

// Only for power management

EXTERN_C void CamDrv_SaveFormat(PCamDrv_Context pCameraCxt)
{
    memcpy(&pCameraCxt->SavedAppFormat, &pCameraCxt->AppFormat,
        sizeof(CamDrv_AppFormat));
}

EXTERN_C void CamDrv_ResumeFormat(PCamDrv_Context pCameraCxt)
{
    int i;
    ECode ec;
    int *pParam = (int *)(&pCameraCxt->SavedAppFormat);

    for (i = 0; i < (int)sizeof(s_fpFormatSet) / 4; i++) {
        CamDrv_SetFormatFunc pSetFormat = s_fpFormatSet[i];
        if (pSetFormat) ec = pSetFormat(pCameraCxt, *pParam);
        pParam++;
    }
}

EXTERN_C void CamDrv_SetDefaultFormat(PCamDrv_Context pCameraCxt)
{
    // Default values for current camera format
    pCameraCxt->AppFormat.PhotoResolution = CAMERA_PR_MEDIUM;
    pCameraCxt->AppFormat.VideoResolution = CAMERA_VR_MEDIUM;
    pCameraCxt->AppFormat.FrameRate = 0; // 0 means using the default value
    pCameraCxt->AppFormat.Zoom = 1;
    pCameraCxt->AppFormat.Brightness = 60;
    pCameraCxt->AppFormat.Contrast = 60;
    pCameraCxt->AppFormat.Sharpness = 0; // Should be revise later
    pCameraCxt->AppFormat.WhiteBalance = CAMERA_WB_AUTO;
    pCameraCxt->AppFormat.ColorMode = CAMERA_CM_COLOR;
    pCameraCxt->AppFormat.SceneMode = CAMERA_SM_AUTO;
    pCameraCxt->AppFormat.Direction = CAMERA_DR_HORIZONTAL;
    pCameraCxt->AppFormat.Flash = CAMERA_FL_AUTO;
    pCameraCxt->AppFormat.ISO = 200;
    pCameraCxt->AppFormat.ExposureCompensation = 0;
    pCameraCxt->AppFormat.ContinuousSnap = 1;


    SensorGetAppResolution(pCameraCxt, CAMERA_PR_MEDIUM);
    SensorGetDrvResolution(pCameraCxt, CAMERA_PR_MEDIUM, 1);
    pCameraCxt->DrvVideoResLevel = CAMERA_PR_LOW;
    pCameraCxt->DrvPhotoResLevel = CAMERA_PR_MEDIUM;
}

// Invoke this function when an application starts to use camera
EXTERN_C ECode CamDrv_Init(PCamDrv_Context pCameraCxt)
{
    ECode ec = NOERROR;

    CamDrv_SetDefaultFormat(pCameraCxt);

    // set sensor info
    pCameraCxt->SensorType = CAMERA_TYPE_PO1200N;

    // Now use YCbCr422 PLANAR only
    pCameraCxt->InputFormat = XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PACKED/*15*/;
    pCameraCxt->OutputFormat = XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR/*16*/;

    pCameraCxt->OutputFormat = XLLP_CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR; // MUST
    pCameraCxt->FrameRate = 0x0;
    pCameraCxt->UsedFrames = DEFAULT_FRAMES; // DMA buffers hold DEFAULT_FRAMES frames

    ec = CamDrv_OpenHardware(pCameraCxt);

    return ec;
}

EXTERN_C void CamDrv_Quit(PCamDrv_Context pCameraCxt)
{
    CamDrv_CloseHardware(pCameraCxt);

    // Free photo buffer if it still exists
    if (pCameraCxt->PhotoBuffer.AllocBufSize > 0) {
        DMAFreeBuffer(pCameraCxt, FALSE);
    }

    // Free preview buffer if allocated
    if (pCameraCxt->PreviewBuffer.AllocBufSize > 0) {
        DMAFreeBuffer(pCameraCxt, TRUE);
    }
}

EXTERN_C ECode CamDrv_SetBrightness(
    PCamDrv_Context pCameraCxt, int Brightness)
{
    ECode ec;

    ec = SensorSetBrightness(Brightness);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.Brightness = Brightness;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetContrast(PCamDrv_Context pCameraCxt, int Contrast)
{
    ECode ec;

    ec = SensorSetContrast(Contrast);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.Contrast = Contrast;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetSharpness(PCamDrv_Context pCameraCxt, int Sharpness)
{
    ECode ec;

    ec = SensorSetSharpness(Sharpness);
    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.Sharpness = Sharpness;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetColorMode(PCamDrv_Context pCameraCxt, int ColorMode)
{
    ECode ec;
    ec = SensorSetColorMode(ColorMode);

    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.ColorMode = ColorMode;
    }
    return ec;
}

EXTERN_C ECode CamDrv_SetSceneMode(PCamDrv_Context pCameraCxt, int SceneMode)
{
    ECode ec;
    ec = SensorSetSceneMode(SceneMode);

    if (SUCCEEDED(ec)) {
        pCameraCxt->AppFormat.SceneMode = SceneMode;
    }
    return ec;
}

EXTERN_C ECode CamDrv_SetFlash(PCamDrv_Context pCameraCxt, int Flash)
{
    pCameraCxt->AppFormat.Flash = Flash;
    return NOERROR;
}

EXTERN_C ECode CamDrv_SetDigitalZoom(PCamDrv_Context pCameraCxt, int zoom)
{
    ECode ec = NOERROR;
    int OrgPreviewBufferSize;
    Boolean bResumePreview = FALSE;

    if (zoom == pCameraCxt->AppFormat.Zoom) return NOERROR;

//    while (pCameraCxt->CaptureStatus == CamDrv_Status_Still) {
//        DzDelay(10000);
//    }
    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
            pCameraCxt->CaptureStatus = CamDrv_Status_PreviewPaused;
            bResumePreview = TRUE;
            CamDrv_StopVideoCapture(pCameraCxt);
    }

    OrgPreviewBufferSize = pCameraCxt->DrvResolution.VideoWidth *
        pCameraCxt->DrvResolution.VideoHeight;
    ec = SensorGetDrvResolution(pCameraCxt,
        pCameraCxt->AppFormat.PhotoResolution, zoom);
    if (FAILED(ec)) goto Exit;

    if (OrgPreviewBufferSize != 0 &&
        (unsigned long)OrgPreviewBufferSize !=
        pCameraCxt->DrvResolution.VideoWidth *
        pCameraCxt->DrvResolution.VideoHeight) {
        // Preview buffer should be changed.
        DMAFreeBuffer(pCameraCxt, TRUE);
    }

    ec = SensorChangeResolution(pCameraCxt,
        pCameraCxt->AppFormat.PhotoResolution, zoom);
    if (FAILED(ec)) goto Exit;

    // zoom maybe a double value, but here we only support int value
    pCameraCxt->AppFormat.Zoom = zoom;
    kprintf("SensorSetDigitalZoom %d,bResumePreview=%d\n", zoom,bResumePreview);
    SensorSetDigitalZoom(zoom);

Exit:
    if (bResumePreview) {
        pCameraCxt->CaptureStatus = CamDrv_Status_Preview;
        CamDrv_StartVideoCapture(pCameraCxt);
    }

    return ec;

}

#define ROUNDBYTES(n) ((unsigned long)n + 3) & (~3)
EXTERN_C ECode CamDrv_SetPhotoResolution(
    PCamDrv_Context pCameraCxt, int Resolution)
{
    ECode ec = NOERROR;
    Boolean bResumePreview = FALSE;

    int OrgPreviewBufferSize;

    if (Resolution == pCameraCxt->AppFormat.PhotoResolution) return ec;

    if (pCameraCxt->CaptureStatus == CamDrv_Status_Preview) {
        pCameraCxt->CaptureStatus = CamDrv_Status_PreviewPaused; // temp status
        bResumePreview = TRUE;
        CamDrv_StopVideoCapture(pCameraCxt);
    }

    // Reset continuous snap & digital zoom
    if (pCameraCxt->AppFormat.ContinuousSnap > 1) {
        CamDrv_SetContinuousSnap(pCameraCxt, 1);
    }
    if (pCameraCxt->AppFormat.Zoom > 1) {
        CamDrv_SetDigitalZoom(pCameraCxt, 1);
    }

    ec = SensorGetAppResolution(pCameraCxt, Resolution);
    if (FAILED(ec)) return ec;

    OrgPreviewBufferSize = pCameraCxt->DrvResolution.VideoWidth *
        pCameraCxt->DrvResolution.VideoHeight;
    ec = SensorGetDrvResolution(pCameraCxt, Resolution,
        pCameraCxt->AppFormat.Zoom);
    if (FAILED(ec)) return ec;

    if ((unsigned long)OrgPreviewBufferSize !=
        pCameraCxt->DrvResolution.VideoWidth *
        pCameraCxt->DrvResolution.VideoHeight) {
        // Preview buffer should be changed.
        DMAFreeBuffer(pCameraCxt, TRUE);
    }

    ec = SensorChangeResolution(pCameraCxt, Resolution,
        pCameraCxt->AppFormat.Zoom);
    if (FAILED(ec)) return ec;

    pCameraCxt->AppFormat.PhotoResolution = Resolution;

    // Buffer size maybe change if resolution is changed,
    // So free the buffer and let it is allocated again later
    if (pCameraCxt->PhotoBuffer.AllocBufSize > 0) {
        CamDrv_ReleaseFrames(pCameraCxt);
    }

    if (bResumePreview) {
        pCameraCxt->CaptureStatus = CamDrv_Status_Preview;
        CamDrv_StartVideoCapture(pCameraCxt);
    }

    DBGOUT(kprintf("Current photo resolution: %d * %d\n",
            pCameraCxt->AppResolution.PhotoWidth,
            pCameraCxt->AppResolution.PhotoHeight));

    DBGOUT(CamDrv_DumpCurrentFormat(&pCameraCxt->AppFormat));
    return ec;
}

EXTERN_C ECode CamDrv_SetAppFormat(PCamDrv_Context pCameraCxt,
    unsigned int nID, int wParam, int lParam)
{
    ECode ec;

    switch (nID) {
    case CAMERA_FORMAT_PHOTO_RESOLUTION:
        ec = CamDrv_SetPhotoResolution(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_ZOOM:
        ec = CamDrv_SetDigitalZoom(pCameraCxt, wParam / lParam);
        break;
    case CAMERA_FORMAT_BRIGHTNESS:
        ec = CamDrv_SetBrightness(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_CONTRAST:
        ec = CamDrv_SetContrast(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_SHARPNESS:
        ec = CamDrv_SetSharpness(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_COLORMODE:
        ec = CamDrv_SetColorMode(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_SCENEMODE:
        ec = CamDrv_SetSceneMode(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_FLASH:
        ec = CamDrv_SetFlash(pCameraCxt, wParam);
        break;
    case CAMERA_FORMAT_CONTINUOUS_SNAP:
        if (wParam <= 0) wParam = 1;
        ec = CamDrv_SetContinuousSnap(pCameraCxt, wParam);
        break;
    default:
        ec = E_NOT_IMPLEMENTED;
        break;
    }

    return ec;
}

EXTERN_C ECode CamDrv_SetContinuousSnap(
    PCamDrv_Context pCameraCxt, int nCotinuousCount)
{
    if (nCotinuousCount == pCameraCxt->AppFormat.ContinuousSnap) return NOERROR;

    if (nCotinuousCount < 1) goto Error;

    switch (pCameraCxt->AppFormat.PhotoResolution) {
        case CAMERA_PR_LOW:
        // Low quality, allow take 1, 2, 3 photos continuously
        if (nCotinuousCount > 3) goto Error;
        break;
        case CAMERA_PR_MEDIUM:
        // Low quality, allow take 1, 2 photos continuously
       if (nCotinuousCount > 2) goto Error;
        break;
        case CAMERA_PR_HIGH:
        // Low quality, only allow take 1 photo at a time
        if (nCotinuousCount > 1) goto Error;
        break;
    }
    pCameraCxt->AppFormat.ContinuousSnap = nCotinuousCount;

    // Buffer size maybe change if nCotinuousCount is changed,
    // So free the buffer and let it is allocated again later
    if (pCameraCxt->PhotoBuffer.AllocBufSize > 0) {
        CamDrv_ReleaseFrames(pCameraCxt);
    }

    return NOERROR;
Error:
    return E_INVALID_ARGUMENT;
}

EXTERN_C ECode CamDrv_GetAppFormat(PCamDrv_Context pCameraCxt,
    unsigned int nID, int *pwParam, int *plParam)
{
    ECode ec = NOERROR;
    DBGOUT(CamDrv_DumpCurrentFormat(&pCameraCxt->AppFormat));

    switch (nID) {
    case CAMERA_FORMAT_PHOTO_RESOLUTION:
        *pwParam = pCameraCxt->AppFormat.PhotoResolution;
        break;
    case CAMERA_FORMAT_PHOTO_GETSIZE:
        *pwParam = pCameraCxt->AppResolution.PhotoWidth;
        *plParam = pCameraCxt->AppResolution.PhotoHeight;
        break;
    case CAMERA_FORMAT_ZOOM:
        *pwParam = pCameraCxt->AppFormat.Zoom;
        *plParam = 1;
        break;
    case CAMERA_FORMAT_BRIGHTNESS:
        *pwParam = pCameraCxt->AppFormat.Brightness;
        break;
    case CAMERA_FORMAT_CONTRAST:
        *pwParam = pCameraCxt->AppFormat.Contrast;
        break;
    case CAMERA_FORMAT_SHARPNESS:
        *pwParam = pCameraCxt->AppFormat.Sharpness;
        break;
    case CAMERA_FORMAT_COLORMODE:
        *pwParam = pCameraCxt->AppFormat.ColorMode;
        break;
    case CAMERA_FORMAT_SCENEMODE:
        *pwParam = pCameraCxt->AppFormat.SceneMode;
        break;
    case CAMERA_FORMAT_FLASH:
        *pwParam = pCameraCxt->AppFormat.Flash;
        break;
    case CAMERA_FORMAT_CONTINUOUS_SNAP:
        *pwParam = pCameraCxt->AppFormat.ContinuousSnap;
        break;
    case CAMERA_FORMAT_PHOTO_GETFRAMESIZE:
        *pwParam = pCameraCxt->AppResolution.PhotoWidth *
            pCameraCxt->AppResolution.PhotoHeight * 2;
        break;
    default:
        ec = E_NOT_IMPLEMENTED;
        break;
    }

    return ec;

}

// Return physical address now.
EXTERN_C ECode CamDrv_GetStillPhoto(
    PCamDrv_Context pCameraCxt, Camera_Photo* pPhoto)
{
    unsigned long PhotoWidth, PhotoHeight;
    const int BytesPerPixel = 2; // For YCbCr422

    if (pCameraCxt->CaptureStatus != CamDrv_Status_Still )
        return E_ACCESS_DENIED;

    pPhoto->Number = pCameraCxt->AppFormat.ContinuousSnap;
    PhotoWidth = pCameraCxt->AppResolution.PhotoWidth;
    PhotoHeight = pCameraCxt->AppResolution.PhotoHeight;

    // Get photos from DMA buffer directly

    // BufferSize may be larger than PhotoWidth * PhotoHeight * BytesPerPixel
    // But now they are actually equal
    pPhoto->SingleBufferSize = PhotoWidth * PhotoHeight * BytesPerPixel;
    pPhoto->pBuffer = (void *)pCameraCxt->PhotoBuffer.BufVirAddrUser[0][0];

    DBGOUT(CamDrv_DumpPhotoInfo(pPhoto));

    return NOERROR;
}
